#include "sensor_config.h"
#include "sensor_electrode.h"
#include "sensor_welectrode.h"
#include "sensor_celectrode.h"
#include "sensor_impedance.h"
#include "sensor_glucose.h"
#if SENSOR_AUTOBASE
#include "sensor_autobase.h"
#endif

// Constant definition
#define GLUCOSE_MIN           ( 30 * PERCISION / 10  )
#define GLUCOSE_PRED_MINUTES     5
#define GLUCOSE_PRED_UPPER    ( 40 * PERCISION / 100 )
#define NOISE_WEIGHT_C1       (  0 * PERCISION / 100 )
#define NOISE_WEIGHT_C2       ( 90 * PERCISION / 100 )
#define GLU_WEIGHT_C1         (  8 * PERCISION )
#define GLU_WEIGHT_C2         (  2 * 10 * 10 * PERCISION )

#define BUFFER_SIZE              4
#define BUFFER_WEIGHT         { 80*PERCISION/100, 13*PERCISION/100, 5*PERCISION/100, 2*PERCISION/100 }
#define WEIGHTED_CAL_START    (  1 * SECONDS_PER_DAY )
#define C0                    ( 15 * PERCISION )
#define C1                    ( 25 * PERCISION / 100 )
#define C2                    (100 * PERCISION )

#define FORCE_CAL_TIME        (  24 * SECONDS_PER_HOUR )

#define ENABLE_NONLINEAR_SG        1
#define ENABLE_NONLINEAR_SG_NEW    1
#define ENABLE_ESA_COMPENSATION    1
#if ENABLE_ESA_COMPENSATION
#define ENABLE_NONLINEAR_SG        0
#define ENABLE_NONLINEAR_SG_NEW    0
#define GLUCOSE_BUF_SIZE           72
#define ESA_COMP_TARGET         ( 48 * PERCISION / 10 )
#define ESA_COMP_FINISH         ( 80 * PERCISION / 10 )
#endif
#if ENABLE_ESA_COMPENSATION
#define NONLINEAR_SG_PT_2       ( 80 * PERCISION / 100 )
#elif ENABLE_NONLINEAR_SG_NEW
#define NONLINEAR_SG_PT_2       ( 80 * PERCISION / 100 )
#else
#define NONLINEAR_SG_PT_2       ( 40 * PERCISION / 100 )
#endif
#if ENABLE_ESA_COMPENSATION
#define NONLINEAR_SG_TIME_1     ( 96 * SECONDS_PER_HOUR )
#define NONLINEAR_SG_TIME_2     (120 * SECONDS_PER_HOUR )
#define NONLINEAR_SG_TIME_END   (168* SECONDS_PER_HOUR )
#elif ENABLE_NONLINEAR_SG
#define NONLINEAR_SG_TIME_1     ( 36 * SECONDS_PER_HOUR )
#define NONLINEAR_SG_TIME_2     ( 68 * SECONDS_PER_HOUR )
#define NONLINEAR_SG_TIME_END   ( 72* SECONDS_PER_HOUR )
#endif
#define G0      0
#define G0_A    30*PERCISION/10
#define G0_D    G0_A - G0
#define G1      20*PERCISION/10
#define G1_A    40*PERCISION/10
#define G1_D    G1_A - G1
#define G2      55*PERCISION/10
#define GE_1     5*PERCISION/10
#define GE_2    10*PERCISION/10

// Private variable definition
static uint32 m_u32_Time = 0,
              m_u32_calTime = 0;
static fixed32 m_f32_sensFactor = 0,
               m_f32_sensOffset = 0,
               m_f32_calFactor = 0,
               m_f32_preCalFactor = 0,
               m_f32_offset = 0,
               m_f32_preOffset = 0,
               m_f32_defaultGlucose = 0,
               m_f32_origGlucose = 0,
               m_f32_predGlucose = 0,
               m_f32_calGlucose = 0,
               m_f32_calTargetGlucose = 0,
               m_f32_calTargetGlucoseEsa = 0,
               m_f32_preGlucose = 0,
               m_f32_naBias = 0;
#if ENABLE_ESA_COMPENSATION || ENABLE_NONLINEAR_SG_NEW
static uint8 m_u8_countLowSg = 0;
static fixed32 m_f32_diffMax = 0;
static fixed32 m_f32_shiftForLow = 0;
static fixed32 m_f32_factorForLow = PERCISION;
#endif
#if ENABLE_ESA_COMPENSATION
static fixed32 m_f32_glucoseBufData[GLUCOSE_BUF_SIZE] = {0};
static fixed32 m_f32_glucoseTempData[GLUCOSE_BUF_SIZE] = {0};
static cycle_buffer m_t_glucoseBuf = {0};
static fixed32 m_f32_esaBase = ESA_COMP_TARGET;
static fixed32 m_f32_esaCompTarget = ESA_COMP_TARGET;
static fixed32 m_f32_esaCompTargetR = 0;
#endif
#if SENSOR_AUTOBASE
static fixed32 m_f32_autobaseGlucose = 0;
static fixed32 m_f32_baseOffset = PERCISION;
static uint8 m_u8_limitScale = 0;
#endif
static logical m_b_calUpdate = L_FALSE;
static uint m_ui_state = 0;
static uint m_ui_postCalCount = 0;

static fixed32 m_f32_calIsigBufData[BUFFER_SIZE] = {0};
static fixed32 m_f32_calGlucBufData[BUFFER_SIZE] = {0};
static cycle_buffer m_t_calIsigBuf = {0};
static cycle_buffer m_t_glucBuf = {0};
#ifdef ENABLE_WEIGHTED_CAL
static sint32 m_s32_bufferWeight[BUFFER_SIZE] = BUFFER_WEIGHT;
#endif

static const cycle_buffer *m_tp_isigBuffer,
                          *m_tp_isigRawBuffer,
                          *m_tp_isigBadBuffer,
                          *m_tp_noiseBuffer;


// Private function declaration
static void PointersInit(void);
static fixed32 Glu_GetAutoCalFactorAt(uint32 u32_time);
static fixed32 Glu_GetTvCalFactorAt(uint32 u32_time);
static fixed32 Glu_GetAutoOffsetAt(uint32 u32_time);
static fixed32 Glu_GetTvOffsetAt(uint32 u32_time);
#ifdef ENABLE_GLUCOSE_PREDICT
static fixed32 PredictGlucose(fixed32 f32_origGlucose);
#endif
static fixed32 NonlinearAdjustCF(fixed32 f32_factor, fixed32 f32_upperScale, fixed32 f32_lowerScale);
static fixed32 NonlinearAdjustScale(fixed32 f32_scale, fixed32 f32_upperScale, fixed32 f32_lowerScale);
#if ENABLE_NONLINEAR_SG || ENABLE_ESA_COMPENSATION
static fixed32 NonlinearAdjustSG(uint32 u32_time, fixed32 f32_glucose, fixed32 f32_bias, logical b_inverse);
static fixed32 NonlinearAdjustSG_Pt(uint32 u32_time);
#endif
#if ENABLE_NONLINEAR_SG
static fixed32 NonlinearAdjustSG_Bias(uint32 u32_time, fixed32 f32_glucose, fixed32 f32_glucoseA);
#endif

// Function definition

void Glu_Initialize( uint32 u32_calTime, fixed32 f32_calFactor, fixed32 f32_offset, fixed32 f32_naBias)
{
    m_u32_calTime = u32_calTime;
    if ( f32_calFactor > 0 )
    {
        m_f32_sensFactor = PERCISION;
        m_f32_calFactor = f32_calFactor;
        m_f32_preCalFactor = f32_calFactor;
    }
    m_f32_sensOffset = 0;
    m_f32_offset = f32_offset;
    m_f32_preOffset = f32_offset;
    m_f32_defaultGlucose = 0;
    m_f32_origGlucose = 0;
    m_f32_predGlucose = 0;
    m_f32_calGlucose = 0;
    m_f32_calTargetGlucose = 0;
    m_f32_calTargetGlucoseEsa = 0;
    m_f32_preGlucose = 0;
    m_f32_naBias = f32_naBias;
    m_ui_state = GLUCOSE_INITIALIZING;
    m_ui_postCalCount = 0;

#if ENABLE_ESA_COMPENSATION || ENABLE_NONLINEAR_SG_NEW
    m_u8_countLowSg = 0;
    m_f32_diffMax = 0;
    m_f32_shiftForLow = 0;
    m_f32_factorForLow = PERCISION;
#endif
#if ENABLE_ESA_COMPENSATION
    Buffer_Init( &m_t_glucoseBuf, m_f32_glucoseBufData, GLUCOSE_BUF_SIZE );
    m_f32_esaBase = ESA_COMP_TARGET;
    m_f32_esaCompTarget = ESA_COMP_TARGET;
    m_f32_esaCompTargetR = 0;
#endif

    PointersInit();
    Buffer_Init( &m_t_calIsigBuf, m_f32_calIsigBufData, BUFFER_SIZE );
    Buffer_Init( &m_t_glucBuf, m_f32_calGlucBufData, BUFFER_SIZE );
}

static void PointersInit(void)
{
#ifdef ENABLE_KALMAN_FILTER
    m_tp_isigBuffer = WE_GetBuffer(BUF_ISIG_FILT);
#else
    m_tp_isigBuffer = WE_GetBuffer(BUF_ISIG_RAW);
#endif
    m_tp_isigRawBuffer = WE_GetBuffer(BUF_ISIG_RAW);
    m_tp_isigBadBuffer = WE_GetBuffer(BUF_ISIG_BAD);
    m_tp_noiseBuffer = WE_GetBuffer(BUF_NOISE_LEVEL);
}

void Glu_SetCalFactor( uint32 u32_calTime, fixed32 f32_calFactor )
{
    m_u32_calTime = u32_calTime;
    m_f32_calFactor = f32_calFactor;
}

void Glu_SetOffset( uint32 u32_calTime, fixed32 f32_offset )
{
    m_u32_calTime = u32_calTime;
    m_f32_offset = f32_offset;
}

fixed32 Glu_GetCalFactor(void)
{
    return m_f32_calFactor;
}

fixed32 Glu_GetTotalCalFactor(void)
{
    fixed32 f32_value = Glu_GetAutoCalFactorAt(m_u32_Time);
//#if (defined ENABLE_GRADUAL_CAL) && (POST_CAL_TIMES > 0)
//    fixed32 f32_calFactor = (m_f32_calFactor * (POST_CAL_TIMES-m_ui_postCalCount) + m_f32_preCalFactor * m_ui_postCalCount) / POST_CAL_TIMES;
//    f32_value = F_MUL( f32_value, f32_calFactor );
//#else
    f32_value = F_MUL( f32_value, m_f32_calFactor );
//#endif
    f32_value = CLIP( f32_value, CAL_FACTOR_UPPER_THRESHOLD, CAL_FACTOR_LOWER_THRESHOLD );
    return f32_value;
}

fixed32 Glu_GetAutoCalFactorAt(uint32 u32_time)
{
    fixed32 f32_value = Glu_GetTvCalFactorAt(u32_time);
    f32_value = F_MUL( f32_value, m_f32_sensFactor );
    return f32_value;
}

fixed32 Glu_GetTvCalFactorAt(uint32 u32_time)
{
    fixed32 f32_calFactor = 0;
    fixed32 f32_hour = u32_time / SECONDS_PER_MINUTE * PERCISION / MINUTES_PER_HOUR;
    fixed32 f32_hour1 = 0;
    fixed32 f32_hour2 = 0;
    fixed32 f32_calFactor1 = 0;
    fixed32 f32_calFactor2 = 0;

    if (u32_time > DP.EXPIRATION_TIME)
    {
        u32_time = DP.EXPIRATION_TIME;
    }

    if (f32_hour < DP.CAL_FACTOR_HOURS2)
    {
        f32_hour1 = 0;
        f32_hour2 = DP.CAL_FACTOR_HOURS2;
        f32_calFactor1 = DP.CAL_FACTOR_1;
        f32_calFactor2 = DP.CAL_FACTOR_2;
    }
    else if (f32_hour < DP.CAL_FACTOR_HOURS3)
    {
        f32_hour1 = DP.CAL_FACTOR_HOURS2;
        f32_hour2 = DP.CAL_FACTOR_HOURS3;
        f32_calFactor1 = DP.CAL_FACTOR_2;
        f32_calFactor2 = DP.CAL_FACTOR_3;
    }
    else if (f32_hour < DP.CAL_FACTOR_HOURS4)
    {
        f32_hour1 = DP.CAL_FACTOR_HOURS3;
        f32_hour2 = DP.CAL_FACTOR_HOURS4;
        f32_calFactor1 = DP.CAL_FACTOR_3;
        f32_calFactor2 = DP.CAL_FACTOR_4;
    }
    else
    {
        f32_hour1 = DP.CAL_FACTOR_HOURS4;
        f32_hour2 = (int32)DP.EXPIRATION_TIME / SECONDS_PER_MINUTE * PERCISION / MINUTES_PER_HOUR;
        f32_calFactor1 = DP.CAL_FACTOR_4;
        f32_calFactor2 = DP.CAL_FACTOR_5;
    }

    if (f32_hour2 == f32_hour1)
    {
        f32_calFactor = f32_calFactor1;
    }
    else
    {
        f32_calFactor = F_MUL( f32_calFactor1, f32_hour2-f32_hour ) + F_MUL( f32_calFactor2, f32_hour-f32_hour1 );
        f32_calFactor = F_DIV( f32_calFactor, f32_hour2-f32_hour1 );
    }

    return f32_calFactor;
}

fixed32 Glu_GetOffset(void)
{
    return m_f32_offset;
}

fixed32 Glu_GetTotalOffset(void)
{
    return Glu_GetAutoOffsetAt(m_u32_Time) + m_f32_offset;
}

fixed32 Glu_GetAutoOffsetAt(uint32 u32_time)
{
    return Glu_GetTvOffsetAt(u32_time) + m_f32_sensOffset;
}

static fixed32 Glu_GetTvOffsetAt(uint32 u32_time)
{
    fixed32 f32_offset = 0;
    fixed32 f32_hour = u32_time / SECONDS_PER_MINUTE * PERCISION / MINUTES_PER_HOUR;
    fixed32 f32_hour1 = 0;
    fixed32 f32_hour2 = 0;
    fixed32 f32_offset1 = 0;
    fixed32 f32_offset2 = 0;

    if (u32_time > DP.EXPIRATION_TIME)
    {
        u32_time = DP.EXPIRATION_TIME;
    }

    if (f32_hour < DP.OFFSET_HOURS2)
    {
        f32_hour1 = 0;
        f32_hour2 = DP.OFFSET_HOURS2;
        f32_offset1 = DP.OFFSET_1;
        f32_offset2 = DP.OFFSET_2;
    }
    else if (f32_hour < DP.OFFSET_HOURS3)
    {
        f32_hour1 = DP.OFFSET_HOURS2;
        f32_hour2 = DP.OFFSET_HOURS3;
        f32_offset1 = DP.OFFSET_2;
        f32_offset2 = DP.OFFSET_3;
    }
    else if (f32_hour < DP.OFFSET_HOURS4)
    {
        f32_hour1 = DP.OFFSET_HOURS3;
        f32_hour2 = DP.OFFSET_HOURS4;
        f32_offset1 = DP.OFFSET_3;
        f32_offset2 = DP.OFFSET_4;
    }
    else
    {
        f32_hour1 = DP.OFFSET_HOURS4;
        f32_hour2 = (int32)DP.EXPIRATION_TIME / SECONDS_PER_MINUTE * PERCISION / MINUTES_PER_HOUR;
        f32_offset1 = DP.OFFSET_4;
        f32_offset2 = DP.OFFSET_5;
    }

    f32_offset = F_MUL( f32_offset1, f32_hour2-f32_hour ) + F_MUL( f32_offset2, f32_hour-f32_hour1 );
    f32_offset = F_DIV( f32_offset, f32_hour2-f32_hour1 );

    return f32_offset;
}

fixed32 Glu_GetNaBias(void)
{
    return m_f32_naBias;
}

logical Glu_IsCalUpdate(void)
{
    return m_b_calUpdate;
}

fixed32 Glu_GetDefaultGlucose(void)
{
    return m_f32_defaultGlucose;
}

fixed32 Glu_GetOrigGlucose(void)
{
    return m_f32_origGlucose;
}

#if SENSOR_AUTOBASE
fixed32 Glu_GetAutoBaseGlucose(void)
{
    return m_f32_autobaseGlucose;
}
#endif

fixed32 Glu_GetGlucose(void)
{
    return m_f32_predGlucose;
}

uint Glu_GetState(void)
{
    return m_ui_state;
}

fixed32 Glu_GetCurrentOrigGlucose(void)
{
    fixed32 f32_isig, f32_factor, f32_glucose;
    f32_isig = Buffer_Get( m_tp_isigBuffer, 0 ) + Glu_GetTotalOffset();
    f32_isig = F_MAX( f32_isig, 0 );
    f32_factor = Glu_GetTotalCalFactor();
    f32_glucose = F_MUL( f32_isig, f32_factor );
    return f32_glucose;
}

fixed32 Glu_GetCurrentGlucose(void)
{
    fixed32 f32_glucose = Glu_GetCurrentOrigGlucose();

#if ENABLE_ESA_COMPENSATION
    fixed32 f32_esaCompTarget = PERCISION - F_MUL(m_f32_esaCompTargetR, 20*PERCISION/100);
    f32_esaCompTarget = F_MUL(m_f32_esaCompTarget, f32_esaCompTarget);

    fixed32 f32_bias = PERCISION - F_DIV(f32_esaCompTarget, ESA_COMP_TARGET);
    f32_bias = F_MUL(f32_bias, -G0_D);
    fixed32 f32_predGlucose = NonlinearAdjustSG(m_u32_Time, f32_glucose, f32_bias, L_FALSE);

    fixed32 f32_pt = NonlinearAdjustSG_Pt(m_u32_Time);
    fixed32 f32_diff = F_MUL(m_f32_diffMax, f32_pt);
    f32_diff = f32_diff * F_MIN(m_u8_countLowSg,6) / 6;
    fixed32 f32_factorForLow = F_DIV(f32_diff, ESA_COMP_TARGET*5/2) + PERCISION;
    fixed32 f32_shiftForLow = f32_esaCompTarget - F_MUL(f32_esaCompTarget - f32_diff, f32_factorForLow);
    f32_glucose = F_MUL(f32_glucose, f32_factorForLow) + f32_shiftForLow;

    if (f32_glucose < f32_predGlucose)
        f32_glucose = (f32_glucose*2 + f32_predGlucose) / 3;
#elif ENABLE_NONLINEAR_SG
#if ENABLE_NONLINEAR_SG_NEW
    if (m_u8_countLowSg > 0)
    {
        f32_glucose = F_MUL(f32_glucose, m_f32_factorForLow) + m_f32_shiftForLow;
        fixed32 f32_predGlucose = NonlinearAdjustSG(m_u32_Time, m_f32_origGlucose, m_f32_naBias, L_FALSE);
        if (f32_glucose < f32_predGlucose)
            f32_glucose = (f32_glucose*2 + f32_predGlucose) / 3;
    }
#else
    f32_glucose = NonlinearAdjustSG(u32_time, f32_glucose, m_f32_naBias, L_FALSE);
#endif
#endif
    return f32_glucose;
}

#if SENSOR_AUTOBASE
fixed32 Glu_GetCurrentAutoBaseGlucose(void)
{
    fixed32 f32_glucose = Glu_GetCurrentGlucose();
    float minsg = (float)GLUCOSE_MIN/100.0 - (float)Glu_GetTotalCalFactor()/100.0 * (float)Glu_GetTotalOffset()/100.0;
    if (minsg < 0)
        minsg = 0;
    float autobaseGlucose = AB_Calc((float)f32_glucose/100.0, 1, minsg, 0);
    if (!rtIsNaNF(autobaseGlucose))
        f32_glucose = (int32)(autobaseGlucose * 100.0);
    return f32_glucose;
}
#endif

void Glu_CalculateGlucose( uint32 u32_time )
{
    fixed32 f32_isig, f32_factor, f32_addGlucose;

    m_u32_Time = u32_time;

    m_b_calUpdate = L_FALSE;
    if (m_ui_postCalCount > 0)
#if SENSOR_AUTOBASE
        Glu_Calibration( u32_time, 0, m_u8_limitScale );
#else
        Glu_Calibration( u32_time, 0 );
#endif

    f32_isig = Buffer_Get( m_tp_isigBuffer, 0 ) + DP.OFFSET_DEFAULT + Glu_GetTvOffsetAt(u32_time);
    f32_isig = F_MAX( f32_isig, 0 );
    f32_factor = Glu_GetAutoCalFactorAt(m_u32_Time);
    f32_factor = F_MUL( f32_factor, DP.CAL_FACTOR_DEFAULT );
    m_f32_defaultGlucose = F_MUL( f32_isig, f32_factor );

    m_f32_sensOffset = Impedance_GetSensOffset(L_TRUE);
    f32_isig = Buffer_Get( m_tp_isigBuffer, 0 ) + Glu_GetTotalOffset();
    f32_isig = F_MAX( f32_isig, 0 );
    m_f32_sensFactor = Impedance_GetSensFactor(L_TRUE);
    f32_factor = Glu_GetTotalCalFactor();
    m_f32_origGlucose = F_MUL( f32_isig, f32_factor );

#if ENABLE_ESA_COMPENSATION
    if (u32_time > SECONDS_PER_HOUR/2 && WE_Assert() && m_f32_origGlucose > 0)
        Buffer_Push( &m_t_glucoseBuf, m_f32_origGlucose);
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_time);
    if (u32_time > INIT_SECONDS - 300 && m_t_glucoseBuf.u32_count > 0 && f32_pt > 0)
    {
        fixed32 f32_mg1 = Buffer_MeanOfMinN(&m_t_glucoseBuf, m_f32_glucoseTempData, 6, 36);
        fixed32 f32_max_glucose = 0;
        if (m_t_glucoseBuf.u32_count == GLUCOSE_BUF_SIZE)
        {
            uint8 i;
            for(i = m_t_glucoseBuf.u32_count-5; i < m_t_glucoseBuf.u32_count; i++)
                f32_max_glucose += m_f32_glucoseTempData[i];
            f32_max_glucose /= 4;
            fixed32 f32_esaCompTargetR = CLIP_NORM(f32_max_glucose, ESA_COMP_FINISH*3/2, ESA_COMP_FINISH);
            if (m_f32_esaCompTargetR <= f32_esaCompTargetR)
                m_f32_esaCompTargetR = f32_esaCompTargetR;
            else if (m_f32_esaCompTargetR > 0)
                m_f32_esaCompTargetR--;
        }
        fixed32 f32_esaCompTarget = PERCISION - F_MUL(m_f32_esaCompTargetR, 20*PERCISION/100);
        f32_esaCompTarget = F_MUL(m_f32_esaCompTarget, f32_esaCompTarget);
        fixed32 f32_mg2 = Buffer_Mean(&m_t_glucoseBuf, 36);
        fixed32 f32_mg3 = Buffer_Mean(&m_t_glucoseBuf, 6);
        fixed32 f32_mg1w = f32_mg2 - F_MAX(f32_mg1 + PERCISION/2, ESA_COMP_TARGET/2);
        f32_mg1w = CLIP(f32_mg1w, 8*PERCISION/10, 2*PERCISION/10);
        f32_mg1 = WeightMean(f32_mg1, f32_mg2, f32_mg1w);
        if (f32_mg3 < f32_mg1)
            m_f32_esaBase = WeightMean(f32_mg1, f32_mg3, PERCISION/2);
        else
            m_f32_esaBase = f32_mg1;
//        m_f32_esaBase = F_MIN(f32_mg1, f32_mg3);
        fixed32 f32_diff = f32_esaCompTarget - m_f32_esaBase;
        if (f32_diff > 0)
        {
            if (f32_diff < 30)
                f32_diff = 0;
            fixed32 f32_rocGlucose = WE_GetCurrent(ROC_RAW);
            f32_rocGlucose = m_f32_origGlucose + F_MUL(f32_rocGlucose, f32_factor);
            if (f32_rocGlucose > ESA_COMP_FINISH)
            {
                f32_diff = CLIP(f32_rocGlucose-ESA_COMP_FINISH, 25, 10);
                m_f32_diffMax -= F_MIN(m_f32_diffMax, f32_diff);
            }
            else if (m_u8_countLowSg < 12)
                m_u8_countLowSg++;
//            else if (m_f32_diffMax == 0)
//                m_f32_diffMax = CLIP(f32_diff / 3, f32_diff, 5);
            else if (m_f32_diffMax < f32_diff)
                m_f32_diffMax += F_MIN(f32_diff - m_f32_diffMax, 5);
            else
                m_f32_diffMax -= F_MIN(m_f32_diffMax - f32_diff, 5);
        }
        else
        {
            if (m_f32_origGlucose < ESA_COMP_FINISH)
            {
                f32_diff = F_MIN(-f32_diff/6, 10);
                m_f32_diffMax -= F_MIN(m_f32_diffMax, f32_diff);
            }
            else
            {
                if (m_u8_countLowSg > 6)
                    m_u8_countLowSg = 6;
                else if (m_u8_countLowSg > 0)
                    m_u8_countLowSg--;
            }
        }
        if (m_u8_countLowSg > 0)
        {
            fixed32 f32_bias = PERCISION - F_DIV(f32_esaCompTarget, ESA_COMP_TARGET);
            f32_bias = F_MUL(f32_bias, -G0_D);
            fixed32 f32_predGlucose = NonlinearAdjustSG(u32_time, m_f32_origGlucose, f32_bias, L_FALSE);

            f32_diff = F_MUL(m_f32_diffMax, f32_pt);
            f32_diff = f32_diff * F_MIN(m_u8_countLowSg,6) / 6;
            m_f32_factorForLow = F_DIV(f32_diff, ESA_COMP_TARGET*5/2) + PERCISION;
            m_f32_shiftForLow = f32_esaCompTarget - F_MUL(f32_esaCompTarget - f32_diff, m_f32_factorForLow);
            m_f32_predGlucose = F_MUL(m_f32_origGlucose, m_f32_factorForLow) + m_f32_shiftForLow;

            if (m_f32_predGlucose < f32_predGlucose)
                m_f32_predGlucose = (m_f32_predGlucose*2 + f32_predGlucose) / 3;
        }
        else
        {
            m_f32_predGlucose = m_f32_origGlucose;
            m_f32_diffMax = 0;
            m_f32_shiftForLow = 0;
            m_f32_factorForLow = PERCISION;
        }
    }
    else
    {
        m_f32_predGlucose = m_f32_origGlucose;
        m_u8_countLowSg = 0;
        m_f32_diffMax = 0;
        m_f32_shiftForLow = 0;
        m_f32_factorForLow = PERCISION;
    }

#elif ENABLE_NONLINEAR_SG
#if ENABLE_NONLINEAR_SG_NEW
#define NONLINEAR_SG_ROC_HIGH   ( 10 * PERCISION / 100 )
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_time);
    m_f32_predGlucose = NonlinearAdjustSG(0, m_f32_origGlucose, m_f32_naBias, L_FALSE);
    if (u32_time > SECONDS_PER_HOUR/2 && f32_pt > 0)
    {
        fixed32 f32_rocGlucose = WE_GetCurrent(ROC_RAW);
        f32_rocGlucose = F_MUL(f32_rocGlucose, f32_factor);

        fixed32 f32_diff = m_f32_predGlucose - F_MAX(m_f32_origGlucose, 15*PERCISION/10);
        if (m_f32_predGlucose > m_f32_origGlucose)
        {
            if (m_u8_countLowSg < 12)
                m_u8_countLowSg++;
            else if (m_f32_diffMax == 0)
                m_f32_diffMax = CLIP(f32_diff / 3, f32_diff, 5);
            else if (m_f32_diffMax < f32_diff)
                m_f32_diffMax += F_MIN(f32_diff - m_f32_diffMax, 5);
            else
            {
                fixed32 f32_diff_f = (PERCISION - CLIP_NORM(f32_diff, PERCISION, 0)) / 2;
                fixed32 f32_roc_f = CLIP_NORM(f32_rocGlucose, NONLINEAR_SG_ROC_HIGH, NONLINEAR_SG_ROC_HIGH/2);
                fixed32 f32_max_roc = F_SQR(f32_diff_f + f32_roc_f);
                f32_max_roc = F_MUL(f32_max_roc, 20);
                m_f32_diffMax -= F_MIN(m_f32_diffMax - f32_diff, f32_max_roc);
            }

//            else if (f32_rocGlucose > NONLINEAR_SG_ROC_HIGH)
//                m_f32_diffMax -= F_MIN(m_f32_diffMax - f32_diff, 20);
//            else if (f32_diff < PERCISION/2 || f32_rocGlucose > NONLINEAR_SG_ROC_HIGH/2)
//                m_f32_diffMax -= F_MIN(m_f32_diffMax - f32_diff, 5);
//            else if (f32_diff < PERCISION || f32_rocGlucose > NONLINEAR_SG_ROC_HIGH/4)
//                m_f32_diffMax -= F_MIN(m_f32_diffMax - f32_diff, 1);
        }
        else
        {
            if (m_u8_countLowSg > 6)
                m_u8_countLowSg = 6;
            else if (m_u8_countLowSg > 1)
                m_u8_countLowSg -= (f32_rocGlucose > NONLINEAR_SG_ROC_HIGH ? 2 : 1);
            else if (m_u8_countLowSg > 0)
                m_u8_countLowSg = 0;
        }
        if (m_u8_countLowSg > 0)
        {
            f32_diff = F_MUL(m_f32_diffMax, f32_pt);
            f32_diff = f32_diff * F_MIN(m_u8_countLowSg,6) / 6;
            m_f32_shiftForLow = f32_diff*3/5;
            f32_factor = F_DIV(f32_diff, G0_D-f32_diff);
            f32_factor = F_MUL(f32_factor, G0_D);
            m_f32_factorForLow = F_DIV(f32_factor, G2)*2/5 + PERCISION;
            m_f32_predGlucose = F_MUL(m_f32_origGlucose, m_f32_factorForLow) + m_f32_shiftForLow;
            fixed32 f32_predGlucose = NonlinearAdjustSG(u32_time, m_f32_origGlucose, m_f32_naBias, L_FALSE);
            if (m_f32_predGlucose < f32_predGlucose)
                m_f32_predGlucose = (m_f32_predGlucose*2 + f32_predGlucose) / 3;
        }
        else
        {
            m_f32_predGlucose = m_f32_origGlucose;
            m_f32_diffMax = 0;
            m_f32_shiftForLow = 0;
            m_f32_factorForLow = PERCISION;
        }
    }
    else
    {
        m_f32_predGlucose = m_f32_origGlucose;
        m_u8_countLowSg = 0;
        m_f32_diffMax = 0;
        m_f32_shiftForLow = 0;
        m_f32_factorForLow = PERCISION;
    }
#else
    m_f32_predGlucose = NonlinearAdjustSG(u32_time, m_f32_origGlucose, m_f32_naBias, L_FALSE);
#endif
#else
    m_f32_predGlucose = m_f32_origGlucose;
#endif
#ifdef ENABLE_GLUCOSE_PREDICT
    f32_addGlucose = PredictGlucose(m_f32_predGlucose);
#else
    f32_addGlucose = 0;
#endif
    m_f32_predGlucose += f32_addGlucose;

    if (u32_time < INIT_SECONDS - SECONDS_PER_HOUR / GLUCOSE_PER_HOUR)
        m_ui_state = GLUCOSE_INITIALIZING;
    else if ( !Electrode_Assert() || (!CE_Assert() && WE_Low()) )
        m_ui_state = GLUCOSE_ERROR;
    else if ( u32_time > SECONDS_PER_DAY && (!WE_Assert() || m_f32_predGlucose <= 0) )
        m_ui_state = GLUCOSE_ERROR;
    else if ( u32_time > 4*SECONDS_PER_HOUR && WE_NoiseHigh() )
        m_ui_state = GLUCOSE_ERROR;
    else if ( u32_time > 4*SECONDS_PER_DAY && WE_Nosens() )
        m_ui_state = GLUCOSE_ERROR;
    else if ( CE_VCounterShorted() && WE_Hyposens() )
        m_ui_state = GLUCOSE_ERROR;
#ifdef ENABLE_IMP_ERROR_CHECK
//    else if ( !Impedance_Assert() )
//        m_ui_state = GLUCOSE_ERROR;
    else if ( u32_time < 4*SECONDS_PER_HOUR && !Impedance_IsCurrentRealInRange() )
        m_ui_state = GLUCOSE_BLANKED;
#endif
    else if ( m_ui_state == GLUCOSE_ERROR )
        m_ui_state = GLUCOSE_BLANKED;
    else if ( Impedance_SensChanged() ||
              ( (!WE_IsRefValid() || !Impedance_Valid()) &&
                (u32_time > m_u32_calTime + FORCE_CAL_TIME)
              )
            )
        m_ui_state = GLUCOSE_RECOMMEND_CAL;
    else
        m_ui_state = GLUCOSE_OK;


#if SENSOR_AUTOBASE
    uint8 u8_ignore = (m_ui_state == GLUCOSE_INITIALIZING || m_ui_state == GLUCOSE_ERROR);
    float minsg = (float)GLUCOSE_MIN/100.0 - (float)Glu_GetTotalCalFactor()/100.0 * (float)Glu_GetTotalOffset()/100.0;
    if (minsg < 0)
        minsg = 0;
    float autobaseGlucose = AB_Calc((float)m_f32_predGlucose/100.0, u8_ignore, minsg, 1);

    if (rtIsNaNF(autobaseGlucose))
    {
        m_f32_autobaseGlucose = m_f32_predGlucose;
        if (!u8_ignore)
            m_ui_state = GLUCOSE_BLANKED;
    }
    else
    {
        m_f32_autobaseGlucose = (int32)(autobaseGlucose * 100.0);
    }
#endif

#if (defined ENABLE_GRADUAL_CAL)
    if (m_ui_postCalCount > 0)
    {
        m_f32_predGlucose = (m_f32_calTargetGlucose * (POST_CAL_TIMES-m_ui_postCalCount) + m_f32_preGlucose * m_ui_postCalCount) / POST_CAL_TIMES;
#if SENSOR_AUTOBASE
        m_f32_autobaseGlucose = (m_f32_calTargetGlucose * (POST_CAL_TIMES-m_ui_postCalCount) + m_f32_preGlucose * m_ui_postCalCount) / POST_CAL_TIMES;
#endif
    }
    else
    {
#if SENSOR_AUTOBASE
        m_f32_preGlucose = m_f32_autobaseGlucose;
#else
        m_f32_preGlucose = m_f32_predGlucose;
#endif
    }
#endif
}

#ifdef ENABLE_GLUCOSE_PREDICT
static fixed32 PredictGlucose(fixed32 f32_origGlucose)
{
    fixed32 f32_addGlucose, f32_GlucoseROC,
            f32_noiseMean, f32_noiseWeight, f32_glucoseWeight;

    if (     WE_CnscNormalSamples() < SAVIT_FILTER_LENGTH || \
         m_tp_isigBuffer->u32_count < SAVIT_FILTER_LENGTH )
        return 0;

    f32_GlucoseROC = ChangePerMinuteFilt( m_tp_isigBuffer );
    f32_GlucoseROC = F_MUL( f32_GlucoseROC, m_f32_calFactor );

    f32_noiseMean = Buffer_Mean( m_tp_noiseBuffer, 8 );
    f32_noiseWeight = Pow( f32_noiseMean+NOISE_WEIGHT_C1, NOISE_WEIGHT_C2) - 640;
    f32_noiseWeight = Exp( f32_noiseWeight );
    f32_noiseWeight = PERCISION - F_MIN( f32_noiseWeight, PERCISION );

    f32_glucoseWeight = f32_origGlucose - GLU_WEIGHT_C1;
    f32_glucoseWeight = F_SQR( f32_glucoseWeight );
    f32_glucoseWeight = F_DIV( f32_glucoseWeight, GLU_WEIGHT_C2 );
    f32_glucoseWeight = Exp( -f32_glucoseWeight );

    f32_addGlucose = f32_GlucoseROC * GLUCOSE_PRED_MINUTES;
    f32_addGlucose = F_MUL( f32_addGlucose, f32_noiseWeight );
    f32_addGlucose = F_MUL( f32_addGlucose, f32_glucoseWeight );
    f32_addGlucose = CLIP( f32_addGlucose, GLUCOSE_PRED_UPPER, -GLUCOSE_PRED_UPPER );

    return f32_addGlucose;
}
#endif

logical Glu_Calibration( uint32 u32_time, fixed32 f32_glucose
#if SENSOR_AUTOBASE
                         , uint8 u8_limitScale
#endif
                        )
{
    fixed32 f32_isig_0 = 0;
    fixed32 f32_isig = 0;
    fixed32 f32_factor_i = 0;
    fixed32 f32_factor = 0;
    fixed32 f32_offset = 0;
    uint8 u8_postCal = 0;
    uint32 i=0;

    if ( /*m_ui_state == GLUCOSE_INITIALIZING*/ m_u32_Time < INIT_SECONDS ||
         m_ui_state == GLUCOSE_ERROR )
    {
        m_ui_postCalCount = 0;
        return L_FALSE;
    }

    if (f32_glucose > 0)
    {
        m_f32_calGlucose = f32_glucose;
        m_f32_calTargetGlucose = f32_glucose;
#if SENSOR_AUTOBASE
        m_f32_preGlucose = m_f32_autobaseGlucose;
#else
        m_f32_preGlucose = m_f32_predGlucose;
#endif
        m_f32_preCalFactor = m_f32_calFactor;
        m_f32_preOffset = m_f32_offset;
        m_ui_postCalCount = 0;
    }
    else if (m_ui_postCalCount > 0)
    {
        u8_postCal = 1;
        f32_glucose = m_f32_calGlucose;
        m_ui_postCalCount--;
    }

#if SENSOR_AUTOBASE
    fixed32 f32_b_ab = Glu_GetTotalOffset();
#endif


#ifdef ENABLE_RAPID_CAL
    uint ui_count = 0;
    int32 i32_value = 0;
    for ( i=0; i<3; i++)
    {
        i32_value = Buffer_Get( Electrode_GetFirBuffer(ELECTRODE_W2), i );
        i32_value = F_MAX(i32_value, DP.ISIG_REF_DEFAULT);
        i32_value = Buffer_Get( Electrode_GetFirBuffer(ELECTRODE_W1), i ) - i32_value;
        if (i32_value > 0)
        {
            f32_isig += i32_value;
            ui_count++;
        }
    }
    i=0;
    if (ui_count > 0)
        f32_isig /= ui_count;
    if (f32_isig <= 0)
        return L_FALSE;
    f32_isig += Glu_GetAutoOffsetAt(u32_time);
#else
    for ( i=0; i<1; i++ )
    {
        f32_isig = Buffer_Get( m_tp_isigBuffer, i );
        if ( f32_isig > 0 && !Buffer_Get( m_tp_isigBadBuffer, i ))
            break;
    }
    if (i >= 1)
        return L_FALSE;
#endif
    f32_isig_0 = f32_isig;

    uint32 u32_isigTime = WE_GetCurrent(ISIG_TIME);
    if ( u32_isigTime >= i*SECONDS_PER_HOUR/SAMPLES_PER_HOUR )
    {
        u32_isigTime -= i*SECONDS_PER_HOUR/SAMPLES_PER_HOUR;
    }

    f32_isig += Glu_GetAutoOffsetAt(u32_isigTime) + DP.OFFSET_DEFAULT;

    if (f32_isig <= 0)
        return L_FALSE;

    f32_factor_i = Glu_GetAutoCalFactorAt(u32_isigTime);
    f32_factor = f32_factor_i;
    f32_isig = F_MUL( f32_isig, f32_factor );
    fixed32 f32_isig_1 = f32_isig;

    f32_isig = F_MUL( f32_isig, DP.CAL_FACTOR_DEFAULT );
#if ENABLE_NONLINEAR_SG
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_isigTime);
#if !ENABLE_NONLINEAR_SG_NEW
    if (f32_pt > 0)
        f32_isig = NonlinearAdjustSG(u32_isigTime, f32_isig, 0, L_FALSE);
    fixed32 f32_glucose_0 = f32_isig;
#endif
#endif

#ifdef ENABLE_WEIGHTED_CAL
    if (u32_time > WEIGHTED_CAL_START)
    {
        if (m_ui_postCalCount == 0)
        {
            Buffer_Push( &m_t_calIsigBuf, f32_isig );
            Buffer_Push( &m_t_glucBuf, f32_glucose );
            m_f32_preCalFactor = m_f32_calFactor;
        }
        else
        {
            Buffer_Replace( &m_t_calIsigBuf, 0, f32_isig );
        }

        fixed32 f32_sum1 = 0;
        fixed32 f32_sum2 = 0;
        fixed32 f32_weight = 0;
        for (i=0; i<m_t_calIsigBuf.u32_count; i++)
        {
            f32_isig = Buffer_Get( &m_t_calIsigBuf, i );
            f32_glucose = Buffer_Get( &m_t_glucBuf, i );

            f32_weight = f32_glucose - C0;
            f32_weight = F_MUL( f32_weight, C1 );
            f32_weight = C2 - F_MUL(f32_weight, f32_weight);
            if ( f32_isig <= 0 || f32_glucose <= 0 || f32_weight <= 0 )
                continue;
            f32_weight = F_MUL( f32_weight, m_s32_bufferWeight[i]);

//            f32_glucose = F_MUL( f32_isig,   f32_glucose );
//            f32_isig    = F_MUL( f32_isig,   f32_isig    );
//            f32_sum1   += F_MUL( f32_weight, f32_glucose );
//            f32_sum2   += F_MUL( f32_weight, f32_isig    );

            f32_glucose = F_MUL( f32_glucose, f32_weight );
            f32_sum1   += F_DIV( f32_glucose, f32_isig) ;
            f32_sum2   += f32_weight;
        }
        if (f32_sum1 <= 0 || f32_sum2 <= 0)
        {
            Buffer_Reset(&m_t_calIsigBuf);
            Buffer_Reset(&m_t_glucBuf);
            return L_FALSE;
        }
        else
        {
            f32_glucose = f32_sum1;
            f32_isig = f32_sum2;
        }
    }
#endif

    if (f32_isig <= 0)
        return L_FALSE;

//    // Limit Factor by isig
    fixed32 f32_preFactorScale = F_DIV(m_f32_preCalFactor, DP.CAL_FACTOR_DEFAULT);
    fixed32 f32_gluFactorScale = CLIP_NORM(f32_isig, 50*PERCISION/10, 0);
    f32_gluFactorScale = F_MAX(f32_gluFactorScale, PERCISION/3);
    fixed32 f32_factorUpper = WeightMean(DP.CAL_FACTOR_UPPER_SCALE, f32_preFactorScale, f32_gluFactorScale);
    fixed32 f32_factorLower = WeightMean(DP.CAL_FACTOR_LOWER_SCALE, f32_preFactorScale, f32_gluFactorScale);
    f32_factorUpper = F_MAX(f32_factorUpper, PERCISION);
    f32_factorLower = F_MIN(f32_factorLower, PERCISION);
//    fixed32 f32_factorUpper = DP.CAL_FACTOR_UPPER_SCALE;
//    fixed32 f32_factorLower = DP.CAL_FACTOR_LOWER_SCALE;

#if SENSOR_AUTOBASE
    fixed32 f32_currentGlucose = u8_limitScale ? Glu_GetCurrentAutoBaseGlucose() : Glu_GetCurrentGlucose();
#else
    fixed32 f32_currentGlucose = Glu_GetCurrentGlucose();
#endif

    f32_glucose = WeightMean(f32_glucose, f32_currentGlucose, 33);

    // WeightMean BG
    f32_gluFactorScale = F_MAX(f32_gluFactorScale, PERCISION/2);
    f32_gluFactorScale = PERCISION + F_MUL(f32_gluFactorScale, 50*PERCISION/100);
    f32_gluFactorScale = Pow(f32_gluFactorScale, PERCISION/(POST_CAL_TIMES+1)) - PERCISION;
//    f32_glucose = WeightMean(f32_currentGlucose, f32_glucose, f32_gluFactorScale);
    fixed32 f32_scaledGlucose = F_DIV(f32_glucose, f32_currentGlucose);
    f32_scaledGlucose = NonlinearAdjustScale(f32_scaledGlucose, PERCISION+f32_gluFactorScale, PERCISION-f32_gluFactorScale);
    f32_scaledGlucose = F_MUL(f32_currentGlucose, f32_scaledGlucose);
    fixed32 f32_minDiff = PERCISION/(POST_CAL_TIMES+1);
    if (f32_glucose >= f32_currentGlucose)
    {
        f32_scaledGlucose = F_MAX(f32_scaledGlucose, f32_currentGlucose + f32_minDiff);
        f32_glucose = F_MIN(f32_glucose, f32_scaledGlucose);
    }
    else
    {
        fixed32 f32_currentOrigGlucose = Glu_GetCurrentOrigGlucose();
        if (f32_glucose < f32_currentOrigGlucose)
        {
            f32_scaledGlucose = F_MIN(f32_scaledGlucose, (f32_glucose + f32_currentOrigGlucose) / 2);
            f32_scaledGlucose = F_MIN(f32_scaledGlucose, f32_currentGlucose - f32_minDiff);
            f32_glucose = F_MAX(f32_glucose, f32_scaledGlucose);
        }
//        if (u8_postCal)
//            f32_minDiff = F_MAX(f32_minDiff, (f32_currentGlucose - f32_currentOrigGlucose) / (m_ui_postCalCount+2));
//        else
//            f32_minDiff = F_MAX(f32_minDiff, (f32_currentGlucose - f32_currentOrigGlucose) / (POST_CAL_TIMES+1));
//        f32_scaledGlucose = F_MIN(f32_scaledGlucose, f32_glucose - f32_minDiff);
//        f32_glucose = F_MAX(f32_glucose, f32_scaledGlucose);
    }
    m_f32_calTargetGlucose = f32_glucose;

#if ENABLE_ESA_COMPENSATION
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_isigTime);
    if (f32_pt > 0)
        {
        if (f32_glucose <= f32_isig)
        {
            m_f32_calTargetGlucoseEsa = f32_glucose;
            m_f32_diffMax = 0;
            m_f32_esaCompTarget = ESA_COMP_TARGET * 1 / 2;
        }
        else
        {
            fixed32 f32_esaCompTargetR = PERCISION - F_MUL(m_f32_esaCompTargetR, 20*PERCISION/100);
            fixed32 f32_esaCompTargetDefault = F_MUL(ESA_COMP_TARGET, f32_esaCompTargetR);
            fixed32 f32_esaCompTarget = F_MUL(m_f32_esaCompTarget, f32_esaCompTargetR);
            fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_isigTime);
            fixed32 f32_diffMax = m_f32_diffMax + f32_esaCompTargetDefault - f32_esaCompTarget;
            f32_diffMax = F_MIN(f32_diffMax, f32_esaCompTargetDefault - m_f32_esaBase);
            f32_diffMax = F_MAX(f32_diffMax, 0);
            fixed32 f32_diff = F_MUL(f32_diffMax, f32_pt);
            f32_diff = f32_diff * F_MIN(m_u8_countLowSg,6) / 6;
            fixed32 f32_factorForLow = F_DIV(f32_diff, ESA_COMP_TARGET*5/2) + PERCISION;
            fixed32 f32_shiftForLow = f32_esaCompTargetDefault - F_MUL(f32_esaCompTargetDefault - f32_diff, f32_factorForLow);

            m_f32_calTargetGlucoseEsa = f32_glucose - f32_shiftForLow;
            m_f32_calTargetGlucoseEsa = F_DIV(m_f32_calTargetGlucoseEsa, f32_factorForLow);
            if (m_f32_calTargetGlucoseEsa < f32_isig)
            {
                fixed32 f32_diff2 = f32_isig - m_f32_calTargetGlucoseEsa;
                m_f32_calTargetGlucoseEsa = f32_isig;

                f32_diff2 = F_DIV(f32_diff2, f32_pt);
//                fixed32 f32_diff3 = F_MUL(f32_diff2, f32_shiftForLow);
//                f32_diff3 = F_DIV(f32_diff3, f32_glucose - m_f32_calTargetGlucoseEsa);
//                fixed32 f32_diffWeight = CLIP_NORM(f32_diff2, ESA_COMP_TARGET, 0);
//                f32_diff2 = WeightMean(f32_diff2, f32_diff3, f32_diffWeight);
                f32_diff2 = F_MIN(f32_diff2, ESA_COMP_TARGET);
//                m_f32_diffMax = F_MAX(f32_diffMax - f32_diff2, 0);

//                m_f32_esaCompTarget = ESA_COMP_TARGET - f32_diff2 * 2 / 5;
                fixed32 f32_diff_scale = PERCISION - F_DIV(f32_glucose, ESA_COMP_TARGET*5/3);
                f32_diff_scale = CLIP(f32_diff_scale, 70, 40);
                fixed32 f32_diff_scale1 = PERCISION + F_DIV(ESA_COMP_TARGET - m_f32_esaCompTarget, 50);
                f32_diff_scale = F_MUL(f32_diff_scale, f32_diff_scale1);
                f32_diff_scale = F_MIN(f32_diff_scale, PERCISION);
                m_f32_esaCompTarget = ESA_COMP_TARGET - F_MUL(f32_diff2, f32_diff_scale);
                m_f32_esaCompTarget = F_MAX(m_f32_esaCompTarget, ESA_COMP_TARGET * 1 / 2);
                f32_esaCompTarget = F_MUL(m_f32_esaCompTarget, f32_esaCompTargetR);
                fixed32 a = F_DIV(PERCISION, ESA_COMP_TARGET*5/2);
                fixed32 b = PERCISION + F_DIV(m_f32_calTargetGlucoseEsa - f32_esaCompTarget, ESA_COMP_TARGET*5/2);
                fixed32 c = m_f32_calTargetGlucoseEsa - f32_glucose;
                m_f32_diffMax = F_MUL(b, b) - F_MUL(a, c)*4;
                m_f32_diffMax = Sqrt(m_f32_diffMax * PERCISION) - b;
                m_f32_diffMax = F_DIV(m_f32_diffMax, a*2);
            }
            else
            {
                m_f32_diffMax = f32_diffMax;
                m_f32_esaCompTarget = ESA_COMP_TARGET;
            }
//            else if (m_f32_esaCompTarget < ESA_COMP_TARGET)
//            {
//                fixed32 f32_diff2 = m_f32_calTargetGlucoseEsa - f32_isig;
//                f32_diff2 = F_DIV(f32_diff2, F_MAX(f32_pt,PERCISION/5));
//                f32_diff2 = F_MIN(f32_diff2, ESA_COMP_TARGET - m_f32_esaCompTarget);
//                m_f32_calTargetGlucoseEsa -= f32_diff2;
//                m_f32_diffMax += f32_diff2;
//                m_f32_esaCompTarget += f32_diff2;
//            }
        }
        f32_glucose = m_f32_calTargetGlucoseEsa;
    }
#endif

    f32_factor = F_DIV( f32_glucose, f32_isig_1 );
    fixed32 f32_factor_orig = f32_factor;
    f32_factor = NonlinearAdjustCF(f32_factor, f32_factorUpper, f32_factorLower);

#if ENABLE_NONLINEAR_SG
#if ENABLE_NONLINEAR_SG_NEW
    fixed32 f32_diff = F_MUL(m_f32_diffMax, f32_pt);
#endif
    fixed32 f32_naBias = m_f32_naBias;
    if (f32_pt > 0)
    {
#if ENABLE_NONLINEAR_SG_NEW
        fixed32 f32_nl_isig = F_MUL(f32_isig, m_f32_factorForLow) + m_f32_shiftForLow;
//        fixed32 f32_maxDiff = NonlinearAdjustSG(0, 15*PERCISION/10, m_f32_naBias, L_FALSE)-15*PERCISION/10;

        if (f32_glucose < G2)
        {
            f32_naBias = NonlinearAdjustSG_Bias(0, f32_isig, f32_glucose);
            f32_naBias = F_MIN(f32_naBias, 0);
        }
        else
        {
            if (m_f32_diffMax > 0)
            {
                f32_naBias = 0;
                f32_currentGlucose = Glu_GetCurrentGlucose();
                if (f32_glucose >= f32_currentGlucose)
                {
                    fixed32 f32_glucoseMin = F_MAX(G2, f32_nl_isig);
                    fixed32 f32_weight = F_DIV(m_f32_diffMax, G0_D);
                    f32_weight = F_MIN(f32_weight, PERCISION);
                    fixed32 f32_glucose1 = (3*PERCISION-f32_weight)/2;
                    f32_glucose1 = F_MUL(f32_glucoseMin, f32_glucose1);
                    fixed32 f32_glucose2 = 3*PERCISION-2*f32_weight;
                    f32_glucose2 = F_MUL(f32_glucoseMin, f32_glucose2);
                    if (f32_glucose > f32_glucose2)
                        return L_FALSE;
                    else if (f32_glucose > f32_glucose1)
                        f32_glucose = WeightMean(f32_glucose1 , f32_glucose, f32_weight);
                    if (f32_glucose < f32_currentGlucose)
                        return L_FALSE;
                }
            }
        }

        if (f32_glucose >= f32_nl_isig)
        {
            f32_diff = F_MUL(f32_diff, f32_nl_isig);
            f32_diff = F_DIV(f32_diff, f32_glucose);
            fixed32 f32_shiftForLow = f32_diff*3/5;
            fixed32 f32_factor_low = F_DIV(f32_diff, G0_D-f32_diff);
            f32_factor_low = F_MUL(f32_factor_low, G0_D);
            f32_factor_low = F_DIV(f32_factor_low, G2)*2/5 + PERCISION;
            f32_glucose -= f32_shiftForLow;
            f32_glucose = F_DIV(f32_glucose, f32_factor_low);
        }
        else if (f32_glucose >= f32_isig)
        {
//            f32_diff = f32_glucose - f32_isig;
            fixed32 f32_diffScale;
            fixed32 f32_diffPre = f32_diff;
            fixed32 f32_diff1 = f32_diff;
            uint8 i;
            for (i=0; i<5; i++)
            {
                f32_diffScale = F_DIV(f32_isig, G0_D-f32_diff);
                f32_diffScale = F_MUL(f32_diffScale, G0_D);
                f32_diffScale = F_DIV(f32_diffScale, G2);
                f32_diffScale = f32_diffScale*2/5 + 3*PERCISION/5;
                f32_diff1 = F_DIV(f32_glucose - f32_isig, f32_diffScale);
                if (i==4 || ABS(f32_diff1-f32_diff) < 10)
                {
                    f32_diff = F_MAX(f32_diff, f32_diff1);
                    break;
                }
                f32_diff = (f32_diff + f32_diff1)/2;
            }

            f32_diff = F_MIN(f32_diffPre, f32_diff);
            f32_glucose = f32_isig;
        }
        else
        {
            f32_diff = 0;
        }

        f32_factor = F_DIV( f32_glucose, f32_isig_1 );
        f32_factor = NonlinearAdjustCF(f32_factor, f32_factorUpper, f32_factorLower);

        f32_isig = F_DIV( f32_glucose, f32_factor );
        f32_isig = F_DIV( f32_isig, f32_factor_i );
        f32_offset = f32_isig - f32_isig_0 - Glu_GetAutoOffsetAt(u32_isigTime);

#else
        if (f32_isig >= G2 && f32_glucose >= G2)
        {
            f32_naBias = 0;
            f32_isig = F_DIV( f32_glucose, f32_factor );
            f32_isig = F_DIV( f32_isig, f32_factor_i );
            f32_offset = f32_isig - f32_isig_0 - Glu_GetAutoOffsetAt(u32_isigTime);
        }
        else
        {
            f32_factor = WeightMean(DP.CAL_FACTOR_DEFAULT, f32_factor, f32_pt);
            f32_isig = F_MUL( f32_isig_1, f32_factor );

            fixed32 f32_scale = PERCISION - CLIP_NORM(f32_isig, G2, G0);
            f32_scale = F_SQR(f32_scale);
            f32_factor = F_DIV( NonlinearAdjustSG(u32_isigTime, f32_glucose, 0, L_TRUE), f32_isig_1 );

            if (f32_glucose_0 >= f32_glucose)
            {
                fixed32 f32_factor_0 = F_DIV( f32_glucose, f32_isig_1 );
                fixed32 f32_scale_2 = PERCISION - CLIP_NORM(f32_glucose_0-f32_glucose, 2*PERCISION, 0);
                f32_scale_2 = F_SQR(f32_scale_2);
                f32_factor = WeightMean(f32_factor, f32_factor_0, f32_scale_2);
                f32_factor = F_MIN(f32_factor, DP.CAL_FACTOR_DEFAULT);
            }

            f32_factor = NonlinearAdjustCF(f32_factor, f32_factorUpper, f32_factorLower);
            f32_factor = WeightMean(DP.CAL_FACTOR_DEFAULT, f32_factor, F_MUL(f32_pt, f32_scale));
            f32_isig = F_MUL( f32_isig_1, f32_factor );

            f32_naBias = NonlinearAdjustSG_Bias(u32_isigTime, f32_isig, f32_glucose);
            if (f32_naBias > 0)
            {
                fixed32 f32_error1 = f32_glucose/10;
                f32_error1 = f32_naBias - CLIP(f32_error1, GE_2, GE_1);
                fixed32 f32_error2 = (f32_glucose-NonlinearAdjustSG(0, f32_isig, 0, L_FALSE))/2;
                f32_error2 = F_MAX(f32_error1, f32_error2);
                f32_naBias = F_MIN(f32_naBias, f32_error2);
            }

            f32_offset = DP.OFFSET_DEFAULT;
        }
#endif
    }
    else
#endif
    {
#if SENSOR_AUTOBASE
        static const fixed32 f32_P = 90 * PERCISION / 100;
        float b0 = AB_GetB0();
        float b = AB_GetB();
        if (!rtIsNaNF(b0) && !rtIsNaNF(b))
        {
            m_f32_baseOffset = (int32)((b0 - b) * PERCISION);
            m_f32_baseOffset = F_MUL(m_f32_baseOffset, f32_P);
        }
        m_u8_limitScale = u8_limitScale;
        if (u8_limitScale)
        {
            fixed32 f32_factorScale = F_DIV((4*PERCISION - ABS(m_f32_baseOffset)), 3*PERCISION);
            f32_factorScale = CLIP(f32_factorScale, PERCISION, 50*PERCISION/100);
            if (m_f32_baseOffset > 0)
            {
                fixed32 upper = PERCISION + F_MUL((DP.CAL_FACTOR_UPPER_SCALE - PERCISION), f32_factorScale);
                upper = F_MIN(upper, f32_factorUpper);
                f32_factor = NonlinearAdjustCF(f32_factor_orig, upper, f32_factorLower);
            }
            else
            {
                fixed32 lower = PERCISION + F_MUL((DP.CAL_FACTOR_LOWER_SCALE - PERCISION), f32_factorScale);
                lower = F_MAX(lower, f32_factorLower);
                f32_factor = NonlinearAdjustCF(f32_factor_orig, f32_factorUpper, lower);
            }
        }
#endif
        f32_isig = F_DIV( f32_glucose, f32_factor );
        f32_isig = F_DIV( f32_isig, f32_factor_i );
        f32_offset = f32_isig - f32_isig_0 - Glu_GetAutoOffsetAt(u32_isigTime);
    }

//    fixed32 f32_minsg = Glu_GetTvCalFactorAt(u32_time);
//    f32_minsg = F_MUL(f32_minsg, f32_factor);
//    f32_minsg = F_MUL(f32_minsg, Glu_GetTvOffsetAt(u32_time) + f32_offset);
//    if (f32_minsg > GLUCOSE_MIN)
//    {
//        return L_FALSE;
//    }
    fixed32 f32_offsetMax = Glu_GetTvCalFactorAt(u32_time);
    f32_offsetMax = F_DIV(GLUCOSE_MIN, f32_offsetMax);
    f32_offsetMax = F_DIV(GLUCOSE_MIN, f32_factor);
    f32_offsetMax -= Glu_GetTvOffsetAt(u32_time);
    f32_offset = F_MIN(f32_offset, f32_offsetMax);

    if (u8_postCal == 0)
    {
        m_ui_postCalCount = POST_CAL_TIMES;
    }
//    else if (m_ui_postCalCount == 0)
    {
        m_f32_calFactor = f32_factor;
        if (m_f32_calFactor > DP.CAL_FACTOR_DEFAULT)
            m_f32_offset = F_MAX(f32_offset, DP.OFFSET_DEFAULT);
        else
            m_f32_offset = F_MIN(f32_offset, DP.OFFSET_DEFAULT);
        m_u32_calTime = u32_time;
#if ENABLE_NONLINEAR_SG
        if (f32_pt > 0)
        {
            m_f32_naBias = f32_naBias;
#if ENABLE_NONLINEAR_SG_NEW
            m_f32_diffMax = F_DIV(f32_diff, f32_pt);
            m_f32_shiftForLow = f32_diff*3/5;
            m_f32_factorForLow = F_DIV(f32_diff, G0_D-f32_diff);
            m_f32_factorForLow = F_MUL(m_f32_factorForLow, G0_D);
            m_f32_factorForLow = F_DIV(m_f32_factorForLow, G2)*2/5 + PERCISION;
        }
#endif
#endif
        m_b_calUpdate = L_TRUE;

#if SENSOR_AUTOBASE
        f32_b_ab = Glu_GetTotalOffset() - f32_b_ab;
        f32_b_ab = F_MUL(f32_b_ab, Glu_GetTotalCalFactor());
        AB_Calib((float)m_f32_calTargetGlucose / PERCISION, (float)f32_b_ab / PERCISION);
#endif
    }

    return L_TRUE;
}

static fixed32 NonlinearAdjustCF(fixed32 f32_factor, fixed32 f32_upperScale, fixed32 f32_lowerScale)
{
    fixed32 f32_scale = F_DIV( f32_factor, DP.CAL_FACTOR_DEFAULT );
    f32_scale = NonlinearAdjustScale(f32_scale, f32_upperScale, f32_lowerScale);
    return F_MUL( f32_scale, DP.CAL_FACTOR_DEFAULT );
}

static fixed32 NonlinearAdjustScale(fixed32 f32_scale, fixed32 f32_upperScale, fixed32 f32_lowerScale)
{
    fixed32 f32_start, f32_range, f32_nonlinear, f32_temp;

    if (f32_scale > PERCISION)
    {
        f32_start = f32_upperScale;
        f32_range = ( f32_start - PERCISION ) / 2;
        if (f32_range == 0)
            return PERCISION;
        f32_nonlinear = 200 * PERCISION / f32_range;
        f32_start -= f32_range;
        if (f32_scale <= f32_start)
            return f32_scale;
    }
    else
    {
        f32_start = f32_lowerScale;
        f32_range = ( PERCISION - f32_start ) / 2;
        if (f32_range == 0)
            return PERCISION;
        f32_nonlinear = 200 * PERCISION / f32_range;
        f32_start += f32_range;
        if (f32_scale >= f32_start)
            return f32_scale;
    }

    f32_temp = f32_scale - f32_start;
    f32_temp = -F_MUL( f32_temp, f32_nonlinear );
    f32_temp = PERCISION + Exp(f32_temp);
    f32_temp = F_DIV( 2*f32_range, f32_temp );
    f32_temp += f32_start - f32_range;

    if (f32_scale > PERCISION)
        f32_scale = F_MIN( f32_temp, f32_scale );
    else
        f32_scale = F_MAX( f32_temp, f32_scale );

    return f32_scale;
}

#if ENABLE_NONLINEAR_SG || ENABLE_ESA_COMPENSATION
static fixed32 NonlinearAdjustSG(uint32 u32_time, fixed32 f32_glucose, fixed32 f32_bias, logical b_inverse)
{
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_time);
    if (f32_pt == 0)
        return f32_glucose;

    fixed32 f32_temp, f32_sg0, f32_sg1, f32_sg2;

    if (f32_bias >= 0)
    {
        f32_temp = G0_D + U_DIV(f32_bias, 2);
        f32_sg0 = G0 + F_MUL(f32_temp, f32_pt);
        f32_temp = G1_D + f32_bias;
        f32_sg1 = G1 + F_MUL(f32_temp, f32_pt);
        f32_sg2 = G2 + F_MUL(f32_bias, f32_pt);
    }
    else
    {
        f32_temp = PERCISION + F_DIV(f32_bias, G0_D);
        f32_temp = F_MUL(G0_D, f32_temp);
        f32_sg0 = G0 + F_MUL(f32_temp, f32_pt);
        f32_temp = PERCISION + F_DIV(f32_bias, G0_D);
        f32_temp = F_MUL(G1_D, f32_temp);
        f32_sg1 = G1 + F_MUL(f32_temp, f32_pt);
        f32_sg2 = G2;
    }

    if (b_inverse)
    {
        if (f32_glucose <= f32_sg0)
        {
            return G0;
        }
        if (f32_glucose <= f32_sg1)
        {
            f32_temp = F_DIV(G1-G0, f32_sg1-f32_sg0);
            f32_temp = F_MUL(f32_glucose-f32_sg0, f32_temp) + G0;
        }
        else
        {
            f32_temp = F_DIV(G2-G1, f32_sg2-f32_sg1);
            f32_temp = F_MUL(f32_glucose-f32_sg1, f32_temp) + G1;
        }
        return F_MIN(f32_glucose, f32_temp);
    }
    else
    {
        if (f32_glucose <= G1)
        {
            f32_temp = F_DIV(f32_sg1-f32_sg0, G1-G0);
            f32_temp = F_MUL(f32_glucose-G0, f32_temp) + f32_sg0;
        }
        else
        {
            f32_temp = F_DIV(f32_sg2-f32_sg1, G2-G1);
            f32_temp = F_MUL(f32_glucose-G1, f32_temp) + f32_sg1;
        }
        return F_MAX(f32_glucose, f32_temp);
    }
}

static fixed32 NonlinearAdjustSG_Pt(uint32 u32_time)
{
    fixed32 f32_pt;
    if (u32_time < NONLINEAR_SG_TIME_1)
    {
        f32_pt = PERCISION;
    }
    else if (u32_time < NONLINEAR_SG_TIME_2)
    {
        f32_pt = F_DIV(u32_time - NONLINEAR_SG_TIME_1, NONLINEAR_SG_TIME_2 - NONLINEAR_SG_TIME_1);
        f32_pt = PERCISION - F_MUL(f32_pt, PERCISION - NONLINEAR_SG_PT_2);
    }
    else if (u32_time < NONLINEAR_SG_TIME_END)
    {
        f32_pt = F_DIV(u32_time - NONLINEAR_SG_TIME_2, NONLINEAR_SG_TIME_END - NONLINEAR_SG_TIME_2);
        f32_pt = NONLINEAR_SG_PT_2 - F_MUL(f32_pt, NONLINEAR_SG_PT_2);
    }
    else
    {
        f32_pt = 0;
    }

    return CLIP(f32_pt, PERCISION, 0);
}
#endif

#if ENABLE_NONLINEAR_SG
static fixed32 NonlinearAdjustSG_Bias(uint32 u32_time, fixed32 f32_glucose, fixed32 f32_glucoseA)
{
    fixed32 f32_pt = NonlinearAdjustSG_Pt(u32_time);
    if (f32_pt == 0 || f32_glucoseA <= f32_glucose)
        return -G0_D;

    fixed32 f32_ret;
    f32_ret = NonlinearAdjustSG(u32_time, f32_glucose, 0, L_FALSE);
    if (f32_glucoseA > f32_ret)
    {
        if (f32_glucose <= G1)
        {
            fixed32 f32_value1 = F_DIV(f32_glucose - G0, G1 - G0);
            fixed32 f32_value2 = F_MUL(f32_value1, G1_D - G0_D) + G0_D;
            f32_value2 = F_DIV(f32_glucoseA - f32_glucose, f32_pt) - f32_value2;
            f32_value2 *= 2;
            f32_value1 += PERCISION;
            f32_ret = F_DIV(f32_value2, f32_value1);
        }
        else
        {
            fixed32 f32_value1 = PERCISION - F_DIV(f32_glucose - G1, G2 - G1);
            f32_ret = F_DIV(f32_glucoseA - f32_glucose, f32_pt) - F_MUL(f32_value1, G1_D);
        }
    }
    else
    {
        if (f32_glucose < G1)
        {
            fixed32 f32_value1 = F_DIV(f32_glucose - G0, G1 - G0);
            f32_value1 = F_MUL(f32_value1, G1_D - G0_D) + G0_D;
            fixed32 f32_value2 = F_DIV(f32_glucoseA - f32_glucose, f32_pt);
            f32_value2 = f32_value1 == 0 ? 0 : (F_DIV(f32_value2, f32_value1) - PERCISION);
            f32_ret = F_MUL(f32_value2, G0_D);
        }
        else
        {
            fixed32 f32_value1 = PERCISION - F_DIV(f32_glucose - G1, G2 - G1);
            f32_value1 = F_MUL(f32_value1, G1_D);
            fixed32 f32_value2 = F_DIV(f32_glucoseA - f32_glucose, f32_pt);
            f32_value2 = f32_value1 == 0 ? 0 : (F_DIV(f32_value2, f32_value1) - PERCISION);
            f32_ret = F_MUL(f32_value2, G0_D);
        }
    }
    return CLIP(f32_ret, 5*PERCISION, -G0_D);
}
#endif
